#ifndef __TANGENTFIELD_H__
#define __TANGENTFIELD_H__

#include "vectorfield.h"
#include "470bot.h"
#include "Vector2.h"
#include "KalmanObstacle.h"

class TangentField :
	public VectorField
{
private:
	KalmanObstacle * _obstacle;
	Vector2 _loc;

	double minDist;
	double maxDist;
	double maxRadius;

	bool lineSegmentIntersect(Vector2 start1, Vector2 end1, Vector2 start2, Vector2 end2, Vector2& intersect);

	double GetDist(Vector2 start, Vector2 end, Vector2 point);
public:
	Vector2 get_trajectory(Vector2 & tank_loc);

	TangentField(KalmanObstacle * _obstacle) {
		this->_obstacle = _obstacle;
		
		minDist = 10;
		maxDist = 20;

		//calculate center
		double x = 0, y = 0;
		for(int i = 0; i < this->_obstacle->corners().size(); i ++){
			Vector2 corner_pos = _obstacle->corners().at(i).GetCurrentEstimate();
			x += corner_pos.x;
			y += corner_pos.y;
		}

		_loc = Vector2(x/(double)this->_obstacle->corners().size(), y/this->_obstacle->corners().size());

		double maxRad = 0;
		int next;
		for(int i = 0; i < this->_obstacle->corners().size(); i++){
			next = i +1;
			if(i == this->_obstacle->corners().size()-1) next = 0;

			Vector2 this_corner_pos = _obstacle->corners().at(i).GetCurrentEstimate();
			Vector2 next_corner_pos = _obstacle->corners().at(next).GetCurrentEstimate();

			Vector2 r (next_corner_pos.x - this_corner_pos.x,
					   next_corner_pos.y - this_corner_pos.y);

			Vector2 n (-r.y, r.x);

			Vector2 p = (r + n).normalize() * maxDist;

			p = Vector2(next_corner_pos.x + p.x, next_corner_pos.y + p.y);

			if(maxRad < (p - _loc).magnitude())
				maxRad = (p - _loc).magnitude();
		}
		maxRadius = maxRad;
	}
	TangentField(VectorField & field, Vector2 & loc)
		: VectorField(field), _loc(loc) {
	}

	~TangentField(void);
};
#endif

